/*
 * CPFSensorModel.h
 *
 *  Created on: Nov 27, 2010
 *      Author: bipins
 */

#ifndef CPFSENSORMODEL_H_
#define CPFSENSORMODEL_H_

#include "constants.h"
#include "MyPose.h"
#include "Robot.h"
#include "library.h"
#include "LaserSensor.h"
#include "CostMarker.h"

class CPFSensorModel {
private:

    MATRIX obstacleCostMap;
    LaserSensor laser;
    CostMarker cm;
	ros::Publisher cost_marker_pub;
	double distanceToClosestPerson(double laserX, double laserY, const PoseArray &personPoses, const PoseArray &personPosesMeans, const vector<DD> &personPosesStds);
	double modifiedEuclideanDistance(double x1, double y1, double stdx, double stdy, double x2, double y2);
    double gaussianProbability(double mean, double variance, double x);


public:
	CPFSensorModel(ros::NodeHandle n);
	virtual ~CPFSensorModel();

	void setObstacleCostMap(const MATRIX &m);
    void estimateWeights(const MyPose &robotPose, const PoseArray &personPoses,
						const PoseArray &personPosesMeans, const vector<DD> &personPosesStds,
						const Robot &robot, double &w);
    void numberOfUnknownObstacles(const MyPose &robotPose, const PoseArray &personPoses,
    					const PoseArray &personPosesMeans, const vector<DD> &personPosesStds,
    					const Robot &robot, int &M, STLPose &newPersonPose, const MATRIX &map);

};


#endif /* CPFSENSORMODEL_H_ */
